#ifndef __WING_FILTER_H
#define __WING_FILTER_H

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/visualization/cloud_viewer.h>

#pragma once

namespace wing_filter{

    void setColorByIntensity(pcl::PointXYZRGB& p, uint8_t intensity)
    {
        p.r = 255;
        p.g = intensity;
        p.b = 0;
    }

};

class FilterParam{
public:
    std::string topic_name;
    double min_angle;
    double max_angle;
    double max_range;
    double min_intensity;
    bool save_cloud;
};

class WingFilter{

public:
    ros::NodeHandle nh;
    ros::Subscriber scan_sub;
    tf::TransformListener tf_listener;
    laser_geometry::LaserProjection projector;
    FilterParam params;
    /* pcl members */
    pcl::PointCloud<pcl::PointXYZ> laser_cloud;
    pcl::visualization::CloudViewer viewer;

public:
    WingFilter();
    void wait_tf(double timeout=5.0);

private:
    void laser_callback(const sensor_msgs::LaserScan::ConstPtr& scan);

};

WingFilter::WingFilter():
viewer("Cloud Viewer")
{
    nh.param<double>("min_angle", params.min_angle, -1.0471975511965976);
    nh.param<double>("max_angle", params.max_angle, 1.0471975511965976);
    nh.param<double>("min_intensity", params.min_intensity, 80.0);
    nh.param<double>("max_range", params.max_range, 5.0);
    nh.param<std::string>("topic_name", params.topic_name, "scan_back");
    nh.param<bool>("save_cloud", params.save_cloud, false);
    scan_sub = nh.subscribe<sensor_msgs::LaserScan>(params.topic_name, 1, &WingFilter::laser_callback, this);
}

/**
 * @brief Wait For tf message 
 */
void WingFilter::wait_tf(double timeout)
{
    // try
    // {
    //     listener.waitForTransform("map", "odom", ros::Time(0), ros::Duration(5.0));
    //     listener.lookupTransform("map", "odom", ros::Time(0), transform);
    // }
    // catch (tf::TransformException ex)
    // {
    //     ROS_ERROR("%s", ex.what());
    //     ros::Duration(1.0).sleep();
    // }
}

void WingFilter::laser_callback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    sensor_msgs::PointCloud2 cloud;
    pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
    pcl::PointCloud<pcl::PointXYZRGB> filtered_cloud;
    
    projector.transformLaserScanToPointCloud("laser_back", *scan, cloud, tf_listener);
    pcl::fromROSMsg(cloud, pcl_cloud);

    /* filter data */
    filtered_cloud.clear();
    filtered_cloud.height = 1;
    filtered_cloud.header = pcl_cloud.header;

    int i_min = 0;
    int i_max = (int)((scan->angle_max-scan->angle_min)/scan->angle_increment);
    if(params.min_angle > scan->angle_min)
        i_min = (int)((params.min_angle-scan->angle_min)/scan->angle_increment);
    if(params.max_angle < scan->angle_max)
        i_max = (int)((params.max_angle-scan->angle_min)/scan->angle_increment);

    for(int i=i_min;i<i_max;i++)
    {
        if(scan->ranges[i]<params.max_range)
        {
            if(scan->intensities[i]>params.min_intensity)
            {
                pcl::PointXYZRGB _p;
                _p.x = pcl_cloud[i].x;
                _p.y = pcl_cloud[i].y;
                _p.z = pcl_cloud[i].z;
                wing_filter::setColorByIntensity(_p, scan->intensities[i]);
                filtered_cloud.push_back(_p);
            }
        }
    }

    if(params.save_cloud)
    {
        pcl::io::savePCDFile(params.topic_name+".pcd", filtered_cloud);
        params.save_cloud = false;
        ROS_INFO_STREAM("Cloud File is Saved.");
        ros::shutdown();
    }

    viewer.showCloud(filtered_cloud.makeShared());
    std::cout << scan->header.stamp << std::endl;
}

#endif
